#include "./arm_motor.h"
#include "../../utils/progress.h"
#include "../../utils/alg.h"
#include "../can_bus.h"
#include "../../rtos/thread.h"

namespace driver {

using namespace utils;

const uint8_t DISABLE_FRAME[] = {
    0xFF, 0xFF, 0xFF, 0xFF,
    0xFF, 0xFF, 0xFF, 0xFD,
};

const uint8_t ENABLE_FRAME[] = {
    0xFF, 0xFF, 0xFF, 0xFF,
    0xFF, 0xFF, 0xFF, 0xFC,
};

const uint8_t ADJ_FRAME[] = {
    0xFF, 0xFF, 0xFF, 0xFF,
    0xFF, 0xFF, 0xFF, 0xFE,
};

constexpr float PI = 3.1415926f;
constexpr uint16_t POSITION_ERR = 5;

// 3600 表示 360°
static uint32_t angle_to_uint32(uint16_t angle) {
    
    union {
        float f;
        uint32_t u;
    } c;
    c.f = ((float) angle) * 2 * PI / 3600.0f;
    return c.u;
}

static float uint_to_float(uint16_t val, float min, float max, int bits) {
    float span = max - min;
    float offset = min;
    int range = (0x01 << bits) - 1;
    return ((float) val) * span / ((float) range) + offset;
}

struct ArmStatusTask : public rtos::Thread<ArmStatusTask, 1024, osPriorityHigh2> {

    static constexpr uint32_t FLAG_START = 0x01;
    static constexpr uint32_t FLAG_STOP = 0x02;
    static constexpr uint32_t FLAG_RESULT = 0x04;

    void start_query() {
        m_flags.set_flags(FLAG_START);
    }

    void stop_query() {
        m_flags.set_flags(FLAG_STOP);
        m_flags.wait_any_flags(FLAG_RESULT);
    }

    void run() {

        for (;;) {
            uint32_t flag = m_flags.wait_any_flags(FLAG_START | FLAG_STOP);
            if (flag & FLAG_START) {
                loop();
            } else if (flag & FLAG_STOP) {
                m_flags.set_flags(FLAG_RESULT);
            }
        }
    }

private:
    void loop() {
        Error err;
        for (;;) {
            err = armMotor->disable();
            if (err) {
                break;
            }

            uint32_t flag = m_flags.wait_any_flags(FLAG_START | FLAG_STOP, 500);
            if (flag & osFlagsError) {
                continue;
            } else if (flag & FLAG_STOP) {
                break;
            }
        }
        m_flags.set_flags(FLAG_RESULT);
    }

    rtos::EventFlags m_flags;
};

inline utils::Object<ArmStatusTask> armStatusTask;

void ArmMotor::status_update() {
    armStatusTask->start_query();
}

Error ArmMotor::disable() {
    Error err = write(M1_ID, DISABLE_FRAME);
    if (err) {
        goto finish;
    }
    osDelay(50);
    err = write(M2_ID, DISABLE_FRAME);
    if (err) {
        goto finish;
    }
finish:
    return err;
}

ArmMotor::ArmMotor() {
    armStatusTask->start();
}

Error ArmMotor::enable() {
    armStatusTask->stop_query();

    Error err = write(M1_ID, ENABLE_FRAME);
    if (err) {
        goto finish;
    }
    osDelay(50);
    err = write(M2_ID, ENABLE_FRAME);
finish:
    return err;
}

Error ArmMotor::adj() {
    armStatusTask->stop_query();

    Error err = write(M1_ID, ADJ_FRAME);
    if (err) {
        goto finish;
    }
    osDelay(50);
    err = write(M2_ID, ADJ_FRAME);
finish:
    return err;
}

static void encode_u32(uint8_t *buf, uint32_t temp) {
    buf[0] = temp;
    buf[1] = temp >> 8;
    buf[2] = temp >> 16;
    buf[3] = temp >> 24;
}

Error ArmMotor::run_motor2(uint16_t p2, uint16_t speed) {
    
    uint8_t buf8[8];
    uint32_t tmp;
    int16_t sp2 = p2;

    armStatusTask->stop_query();
    
    Error err = write(M2_ID, ENABLE_FRAME);
    if (err) {
        goto finish;
    }

    tmp = angle_to_uint32(p2);
    encode_u32(buf8, tmp);
    tmp = angle_to_uint32(speed);
    encode_u32(buf8 + 4, tmp);

    tmp = 0;
    for (;;) {
        err = write(M2_ID, buf8);
        if (err) {
            goto finish;
        }
        osDelay(500);
        tmp += 500;
        
        if (tmp >= RUN_TIMEOUT) {
            err = ec::ARM_MOTOR2_RUN_TIMEOUT;
            goto finish;
        }
        if (alg::abs(sp2, m_m2_info.pos) <= POSITION_ERR) {
            goto finish;
        }
    }

finish:
    return err;
}

Error ArmMotor::run_motor1(uint16_t p1, uint16_t speed) {
    
    uint8_t buf8[8];
    uint32_t tmp;
    int16_t sp1 = p1;

    armStatusTask->stop_query();
    
    Error err = write(M1_ID, ENABLE_FRAME);
    if (err) {
        goto finish;
    }

    tmp = angle_to_uint32(p1);
    encode_u32(buf8, tmp);
    tmp = angle_to_uint32(speed);
    encode_u32(buf8 + 4, tmp);

    tmp = 0;
    for (;;) {
        err = write(M1_ID, buf8);
        if (err) {
            goto finish;
        }
        osDelay(500);
        tmp += 500;

        if (tmp >= RUN_TIMEOUT) {
            err = ec::ARM_MOTOR1_RUN_TIMEOUT;
            goto finish;
        }
        if (alg::abs(sp1, m_m1_info.pos) <= POSITION_ERR) {
            goto finish;
        }
    }

finish:
    return err;
}

Error ArmMotor::run(uint16_t p1, uint16_t p2, uint16_t speed) {
    Error err;
    uint8_t buf8[8];
    uint32_t temp;
    int16_t sp;

    err = enable();
    if (err) {
        goto finish;
    }

    temp = angle_to_uint32(p1);
    encode_u32(buf8, temp);

    temp = angle_to_uint32(speed);
    encode_u32(buf8 + 4, temp);

    err = write(M1_ID, buf8);
    if (err) {
        goto finish;
    }

    osDelay(50);

    temp = angle_to_uint32(p2);
    encode_u32(buf8, temp);

    err = write(M2_ID, buf8);
    if (err) {
        goto finish;
    }

    temp = 0;
    sp = p1;
    for (;;) {
        err = write(M1_ID, ENABLE_FRAME);
        if (err) {
            goto finish;
        }
        osDelay(100);
        temp += 100;
        if (temp >= RUN_TIMEOUT) {
            err = ec::ARM_MOTOR1_RUN_TIMEOUT;
            goto finish;
        }
        if (alg::abs(sp, m_m1_info.pos) <= POSITION_ERR) {
            break;
        }
    }

    temp = 0;
    sp = p2;
    for (;;) {
        err = write(M2_ID, ENABLE_FRAME);
        if (err) {
            goto finish;
        }
        osDelay(100);
        temp += 100;
        if (temp >= RUN_TIMEOUT) {
            err = ec::ARM_MOTOR2_RUN_TIMEOUT;
            goto finish;
        }
        if (alg::abs(sp, m_m2_info.pos) <= POSITION_ERR) {
            break;
        }
    }

finish:
    return err;
}

Error ArmMotor::write(uint16_t id, const uint8_t *buf8) {
    conn::CANFrame frame;
    bool flag = false;
    
    for (uint8_t i = 0; i < 3; i ++) {
        canBus->write_std_frame(id, buf8, 8);
        flag = canBus->poll_arm(frame, 100);
        if (flag) {
            break;
        }
        osDelay(50);
    }

    switch (id) {
        case M1_ID: 
            if (!flag) {
                return ec::ARM_MOTOR1_CONN_TIMEOUT;
            }
            parse_info(frame, m_m1_info); 
            return ec::OK;

        case M2_ID: 
            if (!flag) {
                return ec::ARM_MOTOR2_CONN_TIMEOUT;
            }
            parse_info(frame, m_m2_info); 
            return ec::OK;
    }
    return ec::INVALID;
}

void ArmMotor::parse_info(const conn::CANFrame &frame, ArmInfo &info) {
    info.id = frame.buf[0] & 0x0F;
    info.err = frame.buf[0] >> 4;
    
    uint16_t temp = (frame.buf[1] << 8) + frame.buf[2];

    // 3600表示360°
    info.pos = uint_to_float(temp, -12.5f, 12.5f, 16) / 2.0f / PI * 3600.0f;

    temp = (frame.buf[3] << 4) + (frame.buf[4] >> 4);
    info.speed = uint_to_float(temp, -30, 30, 12) / 2.0f / PI * 360.0f;

    temp = ((frame.buf[4] & 0x0F) << 8) + frame.buf[5];
    info.torque = uint_to_float(temp, -10, 10, 12);

    info.mos_temp = frame.buf[6];
    info.coil_temp = frame.buf[7];
}



}
